clc;
clear;
close all;

A = [-0.313 56.7 0; -0.0139 -0.426 0; 0 56.7 0];
B = [0.232; 0.0203; 0];
C = [0 0 1];
D = [0];

ss_pitch=c2d(ss(A,B,C,D),1);
x=ones(3,1);
k=100;

ref=linspace(0,0,100);
ref=ref.';
ref(30:70)=1;

mpc=InitMPC(ss_pitch,100,0,0);
mpc=MPCSampling(mpc,k,k,[]);
mpc=MPCbound(mpc,createBounds(ss_pitch,k,[-100],[],-1),createBounds(ss_pitch,k,[100],[],1),100,1:k);
mpc=addMPCstate(mpc,zeros(3,1));
mpc=addMPCcontrol(mpc,0);
mpc=addMPCref(mpc,ref);
mpc=addMPCwApply(mpc,[]);
tic
[u,u_pred]=MPC(mpc);
toc
u
u_pred

[y,t]=lsim(ss_pitch,u_pred,1:100);

plot(t,y,1:100,ref,'r')
grid on